#include "Code7_MOTION_FUNCTION.h"
#include "Code0_CONF_MOTION.h"
#include "math.h" 

/*****************************************************************************
*For Motor ID 11 12 13
*The angle limit for not damage
******************************************************************************
*/
#define MAX_ANGLE_11    2600
#define MIN_ANGLE_11		1920
#define MAX_ANGLE_12		3000
#define MIN_ANGLE_12		100
#define MAX_ANGLE_13		3600
#define MIN_ANGLE_13		0

																				
unsigned char WaistAngle_Low = 0;
unsigned char WaistAngle_High = 0;
int WaistAngle = 2048; // the current position of waist 

////
//unsigned char ID12_angle_L, ID12_angle_H = 0;
//unsigned char ID13_angle_L, ID13_angle_H = 0;				 
//unsigned char ID14_speed_L, ID14_speed_H = 0;	

unsigned char MOTOR11_L, MOTOR11_H = 0;
unsigned char MOTOR12_L, MOTOR12_H = 0;
unsigned char MOTOR13_L, MOTOR13_H = 0;
unsigned char MOTOR14_L, MOTOR14_H = 0;
// int MOTION_MODE_YuThesis;


/*******************************************************************************
* Function Name  : Mtn_RotateWaist
* Description    : This Motion is used in the "Basketball" event.
* Input          : Turning angle received from COMP.
* Output         : None
* Return         : None
* Attention		 	 : The unit of turning angle is degree. If turning angle is larger
									 than 30 or less than -30, then this command will be ignored.
									 Besides, the angle of waist is bounded by 1190 to 2906.
*******************************************************************************/	
void Mtn_RotateWaist(float angle)
{
	unsigned char angle_low,angle_high = 0;									
	
//	if((angle <= 30) && (angle >= -30))
//	{ // limit the angle of waist
//	 WaistAngle = WaistAngle - (int)(angle * 14.3);
//
//	 if(WaistAngle < 1190){WaistAngle = 1190;}
//	 if(WaistAngle > 2906){WaistAngle = 2906;}
//	 angle_low  = WaistAngle & 0xff; // (1024 / 300 = 3.413)
//	 angle_high = WaistAngle >> 8;
//	 
//	 Add_motor_job(  253,  10,   0,   0,   0 );
//	 Add_motor_job(    3,  angle_low,  angle_high, 150, 0 );
//	 Add_motor_job(  255,   5,   0,   0,   0 );
//	}
	//printf("Angle: %d \r\n", (int)(angle*100));
	if((angle <= 40) && (angle >= -40))
	{
		WaistAngle = 2048 - (int)(3 * angle / MX106_Resolution);
		angle_low  = WaistAngle & 0x00ff;
		angle_high = (WaistAngle & 0xff00) >> 8;

		Add_motor_job(  253,  10,   0,   0,   0 );
		Add_motor_job(    4,  angle_low,  angle_high, 150, 0 );
		Add_motor_job(  255,   5,   0,   0,   0 );
		//printf("Waist Angle: %d, Low: %d, High: %d\r\n", WaistAngle, (int)angle_low, (int)angle_high);
	}
}


/*******************************************************************************
* Function Name  : Mtn_Throw
* Description    : This Motion is used in the "Basketball" event.
* Input          : Throwing distance received from COMP.
* Output         : None
* Return         : None
* Attention		 	 : The unit of throwing distance is cm. The speed and angle of 
									 some motors are calculated by pre-fitting curve. 
									 Besides, the throwing distance is bounded by 105 to 200.
*******************************************************************************/	
void Mtn_Throw(float throw_distance)
{
  double ID12_angle = 0;// ID 12 motor angle
  //double ID13_angle = 0;//ID 13 motor angle
  double ID14_speed = 0;// ID 14 motor speed

	unsigned char ID12_angle_L, ID12_angle_H = 0;
	//unsigned char ID13_angle_L, ID13_angle_H = 0;		 
  unsigned char ID14_speed_L, ID14_speed_H = 0;		

  throw_distance = (double)(throw_distance);

  if(throw_distance > 160) {throw_distance = 160;}
  else if(throw_distance < 95) {throw_distance = 95;}

   /*
   ID12_angle = 20*(throw_distance - 130)/40 + 792;// linear motor angle with distance 170:130 , angle 812:792
   
   ID14_speed = -0.000166666666667 * pow(throw_distance,4) 
   				+ 0.101666666666628 * pow(throw_distance,3) 
				- 23.233333333324000 * pow(throw_distance,2) 
				+ 2361.333333332300000 * throw_distance 
				- 90059.999999955700000;
    // Seperate to low / high byte
  			ID12_angle_L = (int)(ID12_angle) & 0xff;
  			ID12_angle_H = (int)(ID12_angle) >> 8;

  			ID14_speed_L = (int)(ID14_speed) & 0xff;
  			ID14_speed_H = (int)(ID14_speed) >> 8;

				Add_motor_job(  253,  10,   0,   0,   0 );
		   	Add_motor_job(   11,  30,   8,  28,   0 );
		   	Add_motor_job(   10,  56,   7, 188,   0 );
		   	Add_motor_job(   12,  49,   3, 173,   1 );
		   	Add_motor_job(   14,  74,   3,  55,   1 );
		   	Add_motor_job(  255,   2,   0,   0,   0 );
		   	Add_motor_job(    3,  56,   7,  20,   0 );
		   	Add_motor_job(   11, 120,   8,   9,   0 );
		   	Add_motor_job(   10, 226,  12, 152,   0 );
		   	Add_motor_job(   13,  62,   6,  47,   0 );
		   	Add_motor_job(   14, 204,   3,  50,   0 );
		   	Add_motor_job(  255,  36,   0,   0,   0 );
		   	Add_motor_job(   14, 					 210,   					 2,  ID14_speed_L,   ID14_speed_H);
		   	Add_motor_job(  255,   2,   0,   0,   0 );
		   	Add_motor_job(   10, 130,   3, 132,   3 );
		   	Add_motor_job(   12,	ID12_angle_L,   ID12_angle_H,  					 30,					   0 );
		   	Add_motor_job(   13, 147,  11,  88,   2 );
		   	Add_motor_job(   14,  76,   1, 194,   1 );
		   	Add_motor_job(  255,   6,   0,   0,   0 );
				Add_motor_job(   11,   0,   8,  17,   0 );
		    Add_motor_job(   12,   0,   2, 100,   0 );
		    Add_motor_job(   13,   0,   8, 100,   0 );
		    Add_motor_job(   14,   0,   2,  70,   0 );
		    Add_motor_job(  255,  20,   0,   0,   0 );
		    Add_motor_job(    3,   0,   8,  37,   0 );
		    Add_motor_job(   10,   0,   8, 150,   0 );
				Add_motor_job(  255,  10,   0,   0,   0 );
	*/
	  ID12_angle = -0.0001666667 *pow(throw_distance,3)
							  + 0.0421428571 *pow(throw_distance,2)
							  - 1.2190476191 *throw_distance
							  + 656.8571428577;
		
		ID14_speed =  0.0011388889 *pow(throw_distance,3)
		            - 0.4240476190 *pow(throw_distance,2)
					      + 55.8027777778 *throw_distance 
					      - 2463.7142857143;		 
	   // Seperate to low / high byte
  			
  			ID12_angle_L = (int)(ID12_angle) & 0xff;
  			ID12_angle_H = (int)(ID12_angle) >> 8;

  			ID14_speed_L = (int)(ID14_speed) & 0xff;
  			ID14_speed_H = (int)(ID14_speed) >> 8;

		//Motion of Basketball_near
   Add_motor_job(  253,  10,   0,   0,   0 );
   Add_motor_job(   11,  30,   8,  28,   0 );
   Add_motor_job(   10,  56,   7, 188,   0 );
   Add_motor_job(   12,  49,   3, 173,   1 );
   Add_motor_job(   14,  74,   3,  55,   1 );
   Add_motor_job(  255,   2,   0,   0,   0 );
   Add_motor_job(    3,  56,   7,  19,   0 );
   Add_motor_job(   11, 110,   8,   7,   0 );
   Add_motor_job(   10,  12,  13, 148,   0 );
   Add_motor_job(   12,  72,   3,   3,   0 );
   Add_motor_job(   13, 122,   6,  38,   0 );
   Add_motor_job(   14, 192,   3,  50,   0 );
   Add_motor_job(   20, 128,  12, 114,   0 );
   Add_motor_job(  255,  19,   0,   0,   0 );
   Add_motor_job(   14,  59,   3,   6,   0 );
   Add_motor_job(  255,  17,   0,   0,   0 );
   Add_motor_job(   14,  28,   2,  ID14_speed_L,   ID14_speed_H );
   Add_motor_job(  255,   2,   0,   0,   0 );
   Add_motor_job(    3, 140,   8, 160,   0 );
   Add_motor_job(   11,  40,   8,  22,   0 );
   Add_motor_job(   10,  92,   1, 174,   3 );
   Add_motor_job(   12,  ID12_angle_L,   ID12_angle_H,  23,   0 );
   Add_motor_job(   13,  44,  11, 122,   1 );
   Add_motor_job(   14, 124,   1,  88,   2 );
   Add_motor_job(   20, 150,   5,  45,   2 );
   Add_motor_job(  255,  20,   0,   0,   0 );
//	USART_SendData(DEBUG_PORT, Master_Receive[MASTER_R_CONT_ANGLE_L]);    
//	while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TXE) == RESET)
//	{
//	}					
//	USART_SendData(DEBUG_PORT, Master_Receive[MASTER_R_CONT_ANGLE_H]);    
//	while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TXE) == RESET)
//	{
//	}	
//	if(Motion.ContinueAngle_RX > 0)
//	{					
//		USART_SendData(DEBUG_PORT, 0);    
//		while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TXE) == RESET)
//		{
//		}		
//	}
//	else
//	{			
//		USART_SendData(DEBUG_PORT, 1);    
//		while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TXE) == RESET)
//		{
//	}		
//	}				
//	USART_SendData(DEBUG_PORT, abs((int)Motion.ContinueAngle_RX));    
//	while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TXE) == RESET)
//	{
//	}																	 
	USART_SendData(DEBUG_PORT, (unsigned char)throw_distance);    
	while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TXE) == RESET)
	{
	}												
//
//	USART_SendData(DEBUG_PORT, ID6_angle_L);    
//	while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TXE) == RESET)
//	{
//	}																					
//	USART_SendData(DEBUG_PORT, ID6_angle_H);    
//	while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TXE) == RESET)
//	{
//	}											
//			
//	USART_SendData(DEBUG_PORT, ID6_speed_L);    
//	while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TXE) == RESET)
//	{
//	}
//	USART_SendData(DEBUG_PORT, ID6_speed_H);    
//	while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TXE) == RESET)
//	{
//	}
//
//	USART_SendData(DEBUG_PORT, ID8_speed_L);    
//	while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TXE) == RESET)
//	{
//	}
//	USART_SendData(DEBUG_PORT, ID8_speed_H);    
//	while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TXE) == RESET)
//	{
//	}
//
//	USART_SendData(DEBUG_PORT, ID9_speed_L);    
//	while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TXE) == RESET)
//	{
//	}
//	USART_SendData(DEBUG_PORT, ID9_speed_H);    
//	while(USART_GetFlagStatus(DEBUG_PORT, USART_FLAG_TXE) == RESET)
//	{
//	}
}

void Mtn_Throw_YuThesis( unsigned char MOTION_MODE_YuThesis )		 //----------------------------------------------------------------
{
 //------------------------------------------------------------------------------------------------
/*  double ID12_angle = 0;// ID 12 motor angle
  double ID13_angle = 0;//ID 13 motor angle
  double ID14_speed = 0;// ID 14 motor speed			
*/
	unsigned char ID11_angle_L = 30;	   // need motor's value form HMI   **************remember****************
	unsigned char ID11_angle_H = 8;
	unsigned char ID12_angle_L = 72;
	unsigned char ID12_angle_H = 3;
	unsigned char ID13_angle_L = 164;
	unsigned char ID13_angle_H = 11;
	unsigned char ID13_speed_L = 153;
	unsigned char ID13_speed_H = 1;
	unsigned char ID14_speed_L = 5;
	unsigned char ID14_speed_H = 0;
				 
	int MOTOR11_value =	 (int)( ID11_angle_L + (ID11_angle_H << 8) );
	int MOTOR12_value =	 (int)( ID12_angle_L + (ID12_angle_H << 8) );
	int MOTOR13_value =  (int)( ID13_angle_L + (ID13_angle_H << 8) );

	switch( MOTION_MODE_YuThesis ) {
		case THROW_MODE1:
			ID11_angle_L = MOTOR11_L;
  		ID11_angle_H = MOTOR11_H;
  		ID12_angle_L = MOTOR12_L;
  		ID12_angle_H = MOTOR12_H;
			break;
    case THROW_MODE2:
			ID11_angle_L = MOTOR11_L;
  		ID11_angle_H = MOTOR11_H;
			//ID13_angle_L = MOTOR13_L;
  		//ID13_angle_H = MOTOR13_H;
  		ID13_speed_L = MOTOR13_L;
  		ID13_speed_H = MOTOR13_H;
			//printf("A_L: %d, A_H: %d, S_L: %d, S_H:%d\r\n", ID11_angle_L, ID11_angle_H,ID13_speed_L, ID13_speed_H);
			break;
		case THROW_MODE3:
			ID11_angle_L = MOTOR11_L;
  		ID11_angle_H = MOTOR11_H;
  		ID14_speed_L = MOTOR14_L;
  		ID14_speed_H = MOTOR14_H;
			//printf("A_L: %d, A_H: %d, S_L: %d, S_H:%d\r\n", ID11_angle_L, ID11_angle_H,ID14_speed_L, ID14_speed_H);
			break;
		case THROW_MODE4:
			ID12_angle_L = MOTOR12_L;
  		ID12_angle_H = MOTOR12_H;
  		ID13_angle_L = MOTOR13_L;
  		ID13_angle_H = MOTOR13_H;
			break;
		case THROW_MODE5:
			ID12_angle_L = MOTOR12_L;
  		ID12_angle_H = MOTOR12_H;
  		ID14_speed_L = MOTOR14_L;
  		ID14_speed_H = MOTOR14_H;
			break;
		case THROW_MODE6:
  		ID13_angle_L = MOTOR13_L;
  		ID13_angle_H = MOTOR13_H;
			ID14_speed_L = MOTOR14_L;
  		ID14_speed_H = MOTOR14_H;
			break;
		default:
			break;			  
	}// end switch

	// Protect Motor for not damage
			if( MOTOR11_value > MAX_ANGLE_11 || MOTOR11_value < MIN_ANGLE_11 ) {
				ID11_angle_L = 0x003c;
				ID11_angle_H = 0x0008;		//DEC:2108
			}
			if( MOTOR12_value > MAX_ANGLE_12 || MOTOR12_value < MIN_ANGLE_12 ) {
			  ID12_angle_L = 0x0004;
				ID12_angle_H = 0x0003;		//DEC:772
			}
			if( MOTOR13_value > MAX_ANGLE_13 || MOTOR13_value < MIN_ANGLE_13 ) {
				ID13_angle_L = 0x005c;
				ID13_angle_H = 0x000b;		 //DEC:2908
			}


    // Seperate to low / high byte
/*				ID11_angle_L = MOTOR11_L;
  			ID11_angle_H = MOTOR11_H;
  			ID12_angle_L = MOTOR12_L;
  			ID12_angle_H = MOTOR12_H;
				ID13_angle_L = MOTOR13_L;
  			ID13_angle_H = MOTOR13_H;
  			
				ID14_speed_L = MOTOR14_L;
  			ID14_speed_H = MOTOR14_H;
*/ 
				// Add protect angle
				 Add_motor_job(  253,  10,   0,   0,   0 );
   Add_motor_job(  253,  10,   0,   0,   0 );
   Add_motor_job(   11,  30,   8,  28,   0 );
   Add_motor_job(   10,  56,   7, 188,   0 );
   Add_motor_job(   12,  49,   3, 173,   1 );
   Add_motor_job(   14,  74,   3,  55,   1 );
   Add_motor_job(  255,   2,   0,   0,   0 );
   Add_motor_job(    3,  56,   7,  20,   0 );
   Add_motor_job(   11, 140,   8,  11,   0 );
   Add_motor_job(   10,  12,  13, 156,   0 );
   Add_motor_job(   12,  92,   3,   6,   0 );
   Add_motor_job(   13, 142,   6,  38,   0 );
   Add_motor_job(   14, 152,   3,  50,   0 );
   Add_motor_job(   20, 184,  11,  99,   0 );
   Add_motor_job(  255,  18,   0,   0,   0 );
   Add_motor_job(   14,  59,   3,   5,   0 );
   Add_motor_job(  255,  18,   0,   0,   0 );
   Add_motor_job(   14,  68,   2, ID14_speed_L, ID14_speed_H);
   Add_motor_job(  255,   2,   0,   0,   0 );
   Add_motor_job(   11, ID11_angle_L, ID11_angle_H,  34,   0 );
   Add_motor_job(   10,  66,   2, 101,   3 );
   Add_motor_job(   12, ID12_angle_L, ID12_angle_H,   9,   0 );
   Add_motor_job(   13, ID13_angle_L, ID13_angle_H, ID13_speed_L,   ID13_speed_H );
   Add_motor_job(   14, 144,   1, 144,   1 );
   Add_motor_job(   20,  94,   6, 175,   1 );
   Add_motor_job(  255,   6,   0,   0,   0 );
	 
	 printf("Second:A_L: %d, A_H: %d, S_L: %d, S_H:%d\r\n", ID11_angle_L, ID11_angle_H,ID13_speed_L, ID13_speed_H);

				/*Add_motor_job(  253,  10,   0,   0,   0 );
		   	Add_motor_job(   11,  30,   8,  28,   0 );
		   	Add_motor_job(   10,  56,   7, 188,   0 );
		   	Add_motor_job(   12,  49,   3, 173,   1 );
		   	Add_motor_job(   14,  74,   3,  55,   1 );
		   	Add_motor_job(  255,   2,   0,   0,   0 );
		   	Add_motor_job(    3,  56,   7,  20,   0 );
		   	Add_motor_job(   11, 120,   8,   9,   0 );
		   	Add_motor_job(   10, 226,  12, 152,   0 );
		   	Add_motor_job(   13,  62,   6,  47,   0 );
		   	Add_motor_job(   14, 204,   3,  50,   0 );
		   	Add_motor_job(  255,  36,   0,   0,   0 );
		   	Add_motor_job(   14, 					 210,   					 2,  ID14_speed_L,   ID14_speed_H);
		   	Add_motor_job(  255,   2,   0,   0,   0 );
		   	Add_motor_job(   10, 130,   3, 132,   3 );
		   	Add_motor_job(   12,	ID12_angle_L,   ID12_angle_H,  					 30,					   0 );
		   	Add_motor_job(   13,	ID13_angle_L,   ID13_angle_H,  					 88,   					 2 );
		   	Add_motor_job(   14,  76,   1, 194,   1 );
		   	Add_motor_job(  255,   6,   0,   0,   0 );
				Add_motor_job(   11,   0,   8,  17,   0 );
		    Add_motor_job(   12,   0,   2, 100,   0 );
		    Add_motor_job(   13,   0,   8, 100,   0 );
		    Add_motor_job(   14,   0,   2,  70,   0 );
		    Add_motor_job(  255,  20,   0,   0,   0 );
		    Add_motor_job(    3,   0,   8,  37,   0 );
		    Add_motor_job(   10,   0,   8, 150,   0 );
				Add_motor_job(  255,  10,   0,   0,   0 );
				*/
}						 


/*******************************************************************************
* Function Name  : MtnC_Waist_F
* Description    : This Motion is used in the continuous motion - "MtnC_Walk_F".
* Input          : None
* Output         : None
* Return         : None
* Attention		 	 : None
*******************************************************************************/	
void MtnC_Waist_F(void)
{						 
	Add_motor_job(	255,   5,		0,	 0,		0 );
	Add_motor_job(    2,  56,   7,  88,   0 );			 
	Add_motor_job(	255,  15,		0,	 0,		0 );
}
																						
/*******************************************************************************
* Function Name  : MtnC_Waist_B
* Description    : This Motion is used in the continuous motion - "MtnC_Walk_B".
* Input          : None
* Output         : None
* Return         : None
* Attention		 	 : None
*******************************************************************************/	
void MtnC_Waist_B(void)
{										 
	Add_motor_job(	255,   5,		0,	 0,		0 );
	Add_motor_job(    2,  46,   7,  88,   0 );				
	Add_motor_job(	255,  15,		0,	 0,		0 ); 
}
